#include <control.h>
#include <string.h>	
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

Client::Client(int fileDescriper, sockaddr_in * arg)
{
	addr = arg;
	fd = fileDescriper;
	int ret = connect(fd, (struct sockaddr*)addr, sizeof(*addr));
	if (ret == -1)
	{
		perror("connect");
		exit(0);
	}
}


Client::Client()
{
	addr = nullptr;
	fd = -1;
}

DroneState::DroneState()
{
	DroneNumber = 0;
	acceleration = std::vector<double>(3, 0);
	position = std::vector<double>(3, 0);
	orientation = std::vector<double>(4, 0);
	velocity = std::vector<double>(3, 0);
	mode = "";
	bridgeMessage = "";
}

void getMsg(const std::string& recvMsg, std::vector<DroneState>& swarmsMsg)
{
	Json::Reader reader;
	Json::Value recv;
	if (reader.parse(recvMsg.c_str(), recv))
	{
		int swarmNumbers = recv["swarmSize"].asInt();
		Json::Value vec = recv["drones"];
		swarmsMsg.resize(swarmNumbers);
		for (int i = 0; i < swarmNumbers; ++i) {
			Json::Value data = vec[i];
			swarmsMsg[i].DroneNumber = data["droneNumber"].asInt();
			swarmsMsg[i].position[0] = data["positionX"].asDouble();
			swarmsMsg[i].position[1] = data["positionY"].asDouble();
			swarmsMsg[i].position[2] = data["positionZ"].asDouble();
			swarmsMsg[i].velocity[0] = data["velocityX"].asDouble();
			swarmsMsg[i].velocity[1] = data["velocityY"].asDouble();
			swarmsMsg[i].velocity[2] = data["velocityZ"].asDouble();
			swarmsMsg[i].mode = data["mode"].asString();
			swarmsMsg[i].bridgeMessage = data["bridgeMessage"].asString();
		}
	}
	else {
		std::cout << "convert fails..." << std::endl;
	}
	return;
}


std::string controlVec2Json(std::vector<ControlMsg>& droneControlVec)
{

    // for(int i=0;i<droneControlVec.size();++i){
    //     std::cout<<droneControlVec[i]<<std::endl;
    // }
    Json::Value controlVec;
	Json::Value control;
	for (int i = 0; i < droneControlVec.size(); ++i) {
		if (droneControlVec[i].droneNumber == -1) {
			continue;
		}
		else {
			control["droneNumber"] = droneControlVec[i].droneNumber;
			control["positionX"] = droneControlVec[i].position[0];
			control["positionY"] = droneControlVec[i].position[1];
			control["positionZ"] = droneControlVec[i].position[2];
			control["mode"] = droneControlVec[i].mode;
            controlVec.append(control);
		}
	}
	
	Json::FastWriter fw;
	return fw.write(controlVec).c_str();
}

void Client::sendMsg(std::vector<ControlMsg>& controlVec)
{
	std::string sendString = controlVec2Json(controlVec);

    // std::cout<<"==="<<std::endl;
    // std::cout<<sendString<<std::endl;
	sendString.append(1,'\0');
	int actualLength = sendString.size();
	int sendLength = actualLength + sizeof(int);
	char sbuf[sendLength];
	memcpy(sbuf, &actualLength,  sizeof(int));
	memmove(sbuf+sizeof(int), sendString.c_str(), sendString.size());
	int byteNum = write(fd, sbuf, sendLength);
	if (byteNum != sendLength) {
	    std::cout << "Error when write to send buffer!" << std::endl;
	}
	return;
}

void Client::refresh()
{
	std::string sendString = "REFRESH";
	sendString.append(1,'\0');
	int actualLength = sendString.size();
	int sendLength = actualLength + sizeof(int);
	char sbuf[sendLength];
	memcpy(sbuf, &actualLength,  sizeof(int));
	memmove(sbuf+sizeof(int), sendString.c_str(), sendString.size());
	int byteNum = write(fd, sbuf, sendLength);
	if (byteNum != sendLength) {
	    std::cout << "Error when write to send buffer!" << std::endl;
	}
	return;
}


int Client::recvMsg(std::vector<DroneState>& recvMsg)
{
	char buf[sizeof(int)];
	int len = read(fd, buf, sizeof(int));
	if (len == sizeof(int))
	{	
		int recvLength;
		memcpy(&recvLength, buf, sizeof(int));
		char rbuf[recvLength];
		char* cur = rbuf;
		int rlen = read(fd, cur,recvLength);
		while(rlen<recvLength){
		recvLength-=rlen;
		cur+=rlen;
		rlen = read(fd,cur,recvLength);
		}
		getMsg(rbuf, recvMsg);
	}
	else if (len == 0)
	{
		return 0;
	}
	else
	{
		return -1;
	}
	return len;
}




std::ostream & operator<<(std::ostream & os, const ControlMsg & msg)
{
	os << "droneNumber is:" << msg.droneNumber << std::endl;
	os << "mode is:" << msg.mode << std::endl;
	os << "position is:(" << msg.position[0] << "," << msg.position[1] << "," << msg.position[2] << ")" << std::endl;
	return os;
}

ControlMsg::ControlMsg()
{
	droneNumber = -1;
	position = std::vector<double>(3, 0);
	mode = "LANDING";
}

ControlMsg::ControlMsg(int drone_number)
{
	droneNumber = drone_number;
	position = std::vector<double>(3, 0);
	mode = "LANDING";
}

ControlMsg::ControlMsg(int drone_number, std::string setMode)
{
	droneNumber = drone_number;
	position = std::vector<double>(3, 0);
	mode = setMode;
}


